#include "WaveForm.h"

#include "math.h"
#include <limits>

#define M_PI 3.14159265358979323846f

WaveForm::WaveForm()
{
}


WaveForm* WaveForm::readFromRaw( const char* fileName )
{
	WaveForm* waveForm = new WaveForm();
	readSamplesFromRaw( fileName, waveForm->getSamples() );
	return waveForm;
}

WaveForm* WaveForm::readFromWave( const char* fileName )
{
	WaveForm* waveForm = new WaveForm();
	readSamplesFromWave( fileName, waveForm->getSamples() );
	return waveForm;
}

WaveForm* WaveForm::createFromSinusoid( std::size_t numSamples, float numCycles, float amplitude )
{
	WaveForm* waveForm = new WaveForm();
	createSinusoid( numSamples, numCycles, amplitude, waveForm->getSamples() );
	return waveForm;
}

void WaveForm::getNormalizedSamples( NormalizedSamples& normalizedSamples ) const
{
	const Samples& samples = getSamples();
	normalizeSamples( samples, normalizedSamples );
}

//
// Static methods
//
void WaveForm::createSinusoid( std::size_t numSamples, float numCyles, float amplitude, Samples& samples )
{
	samples.resize( numSamples );
	for ( std::size_t i=0; i<numSamples; ++i )
	{
		float sample = sin( i * 2.f * M_PI / (numSamples/numCyles));
		sample *= amplitude;
		samples[i] = static_cast<SampleType>( sample * std::numeric_limits<SampleType>::max() );
	}
}

bool WaveForm::readSamplesFromRaw( const char* fileName, Samples& samples )
{
	samples.clear();

	FILE* file = fopen( fileName, "rb" );
	if ( !file )
		return false;

	bool ret = true;
	fseek( file, 0, SEEK_END );
	int fileSize = ftell(file);
	fseek( file, 0, SEEK_SET );
	
	samples.resize( fileSize / sizeof(SampleType) );
	int numRead = fread( &samples[0], fileSize, 1, file );
	if ( numRead!=1 )
		ret = false;

	fclose( file );
	return ret;
}

bool WaveForm::writeSamplesToRaw( const char* fileName, const Samples& samples )
{
	FILE* file = fopen( fileName, "wb" );
	if ( !file )
		return false;

	bool ret = true;
	if ( samples.size()>0 )
	{
		int numWrite = fwrite( &samples[0], samples.size()*sizeof(SampleType), 1, file );
		if ( numWrite!=1 )
			ret = false;
	}
	fclose( file );

	return ret;
}

bool WaveForm::readSamplesFromWave( const char* fileName, Samples& samples )
{
	samples.clear();

	FILE* file = fopen( fileName, "rb" );
	if ( !file )
		return false;

	fseek( file, 0, SEEK_END );
	int fileSize = ftell(file);
	fseek( file, 0, SEEK_SET );
	/////

	bool ret = true;
	ret = ret && fileSize>44;
	
	// see https://ccrma.stanford.edu/courses/422/projects/WaveFormat/
	unsigned long int longInt = 0;
	unsigned short int shortInt = 0;

	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==0x46464952; 	
	}

	if ( ret )
	{
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==fileSize-8; 	
	}

	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==0x45564157;   	
	}

	// Format chunk
	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==0x20746d66;				// "fmt "	
	}

	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==16;	
	}

	if ( ret )
	{
		shortInt = 0;
		fread( &shortInt, sizeof(shortInt), 1, file );
		ret = ret && shortInt==1;			// Linear quantization
	}

	if ( ret )
	{
		shortInt = 0;
		fread( &shortInt, sizeof(shortInt), 1, file );
		ret = ret && shortInt==1;			// Mono
	}

	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==8000;	
	}

	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==8000 * 1 * 16/8;		// Byte rate
	}

	if ( ret )
	{
		shortInt = 0;
		fread( &shortInt, sizeof(shortInt), 1, file );
		ret = ret && shortInt==1 * 16/8;			// Block align
	}

	if ( ret )
	{
		shortInt = 0;
		fread( &shortInt, sizeof(shortInt), 1, file );
		ret = ret && shortInt==16;			// Bits per sample
	}

	// Data chunk
	if ( ret )
	{
		longInt = 0;
		fread( &longInt, sizeof(longInt), 1, file );
		ret = ret && longInt==0x61746164;				// "data"	
	}
	
	if ( ret )
	{
		unsigned long int samplesSizeInBytes = 0;
		fread( &samplesSizeInBytes, sizeof(samplesSizeInBytes), 1, file );
	
		samples.resize( samplesSizeInBytes / sizeof(SampleType) );
		int numRead = fread( &samples[0], samplesSizeInBytes, 1, file );
		if ( numRead!=1 )
			ret = false;
	}

	fclose( file );
	return ret;
}

bool WaveForm::writeSamplesToWave( const char* fileName, const Samples& samples )
{
	FILE* file = fopen( fileName, "wb" );
	if ( !file )
		return false;

	unsigned long int samplesSizeInBytes = samples.size()*sizeof(SampleType);
	bool ret = true;
	
	// see https://ccrma.stanford.edu/courses/422/projects/WaveFormat/
	std::string string;
	unsigned long int longInt = 0;
	unsigned short int shortInt = 0;
	
	string = "RIFF";
	fwrite( string.c_str(), string.size(), 1, file );
	

	longInt = 36 + samplesSizeInBytes;
	fwrite( &longInt, sizeof(longInt), 1, file );
	
	string = "WAVE";
	fwrite( string.c_str(), string.size(), 1, file );
	
	// Format chunk
	string = "fmt ";
	fwrite( string.c_str(), string.size(), 1, file );
	
	longInt = 16;
	fwrite( &longInt, sizeof(longInt), 1, file );
	
	// Linear quantization
	shortInt = 1;
	fwrite( &shortInt, sizeof(shortInt), 1, file );
	
	// Mono
	shortInt = 1;		
	fwrite( &shortInt, sizeof(shortInt), 1, file );

	// Sample rate
	longInt = 8000;
	fwrite( &longInt, sizeof(longInt), 1, file );
	
	// Byte rate
	longInt = 8000 * 1 * 16/8;
	fwrite( &longInt, sizeof(longInt), 1, file );

	// Block align
	shortInt = 1 * 16/8;		
	fwrite( &shortInt, sizeof(shortInt), 1, file );

	// Bits per sample
	shortInt = 16;		
	fwrite( &shortInt, sizeof(shortInt), 1, file );

	// Data chunk
	string = "data";
	fwrite( string.c_str(), string.size(), 1, file );
	
	// Data size in bytes
	longInt = samplesSizeInBytes;
	fwrite( &longInt, sizeof(longInt), 1, file );

	fwrite( &samples[0], samplesSizeInBytes, 1, file );

	fclose( file );

	return ret;
}


void WaveForm::normalizeSamples( const Samples& samples, NormalizedSamples& normalizedSamples )
{
	std::size_t numSamples = samples.size();
	normalizedSamples.resize( numSamples );
	for ( std::size_t i=0; i<numSamples; ++i )
		normalizedSamples[i] = (float)samples[i] / (float)std::numeric_limits<short int>::max();
}

void WaveForm::unnormalizeSamples( const NormalizedSamples& normalizedSamples, Samples& samples )
{
	std::size_t numSamples = normalizedSamples.size();
	samples.resize( numSamples );
	for ( std::size_t i=0; i<numSamples; ++i )
		samples[i] = static_cast<short int>(  (float)normalizedSamples[i] * (float)std::numeric_limits<short int>::max() );
}

void WaveForm::calculateDST( const NormalizedSamples& normalizedSamples, std::vector<float>& dst )
{
	std::size_t numSamples = normalizedSamples.size();
	dst.resize( numSamples );
	
	std::size_t bin;
	std::size_t k;
	float arg;
	for (bin=0; bin<numSamples; bin++) 
	{
		dst[bin] = 0.f;
		for (k=0; k<numSamples; k++) 
		{
			arg = (float)bin * M_PI *(float)k / (float)numSamples;
			dst[bin] += normalizedSamples[k] * sin(arg);
		}
	}
} 

void WaveForm::calculateDFT( const NormalizedSamples& normalizedSamples, FFTBins& fftBins )
{
	std::size_t numSamples = normalizedSamples.size();
	fftBins.resize( numSamples/2 );
	
	std::size_t bin;
	std::size_t k;
	float arg;
	float sign = -1.f; /* sign = -1 -> FFT, 1 -> iFFT */

	for (bin=0; bin<numSamples/2; bin++)	// JBM: was <= here
	{
		fftBins[bin].first = 0.f;
		fftBins[bin].second = 0.f;
		for (k=0; k<numSamples; k++) 
		{
			arg = 2.f*(float)bin*M_PI*(float)k / (float)numSamples;

			fftBins[bin].first += normalizedSamples[k] * sign * sin(arg);
			fftBins[bin].second += normalizedSamples[k] * cos(arg);
		}
	}
}

/*#define M_PI 3.14159265358979323846

long bin;
for (bin = 0; bin <= transformLength/2; bin++) {
	
	
	frequency[bin] = (float)bin * sampleRate / (float)transformLength;
	magnitude[bin] = 20. * log10( 2. * sqrt( sinPart[bin] * sinPart[bin] +
											 cosPart[bin] * cosPart[bin]) / 
											(float)transformLength);
	phase[bin] = 180.*atan2(sinPart[bin], cosPart[bin]) / M_PI - 90.;
}

*/

void WaveForm::calculateFFT( const NormalizedSamples& normalizedSamples, FFTBins& fftBins )
{
	std::size_t numSamples = normalizedSamples.size();
	std::vector<float> fftBuffer;
	fftBuffer.resize( 2 * numSamples );
	
	long logN = (long)(log((float)numSamples)/log(2.)+.5);
	if ( 1 << logN != numSamples )
		return;

	for ( std::size_t i=0; i<numSamples; ++i )
	{
		fftBuffer[i*2+0] = normalizedSamples[i];
		fftBuffer[i*2+1] = 0;
	}
	smbFft( &fftBuffer[0], numSamples, -1 );

	fftBins.resize( numSamples/2 );
	for ( std::size_t i=0; i<numSamples/2; ++i )
	{
		fftBins[i].first = fftBuffer[i*2+0];
		fftBins[i].second = fftBuffer[i*2+1];
	}
}

void WaveForm::calculateInverseFFT( const FFTBins& fftBins, NormalizedSamples& normalizedSamples )
{
	std::size_t numSamples = fftBins.size() * 2;
	
	std::vector<float> fftBuffer;
	fftBuffer.resize( numSamples * 2 );
	
	long logN = (long)(log((float)numSamples)/log(2.)+.5);
	if ( 1 << logN != numSamples )
		return;

	for ( std::size_t i=0; i<numSamples/2; ++i )
	{
		fftBuffer[i*2+0] = fftBins[i].first;
		fftBuffer[i*2+1] = fftBins[i].second;
	}
	for ( std::size_t i=1; i<numSamples/2; ++i )
	{
		fftBuffer[(numSamples-i)*2+0] = fftBuffer[i*2+0];
		fftBuffer[(numSamples-i)*2+1] = -fftBuffer[i*2+1];
	}

	smbFft( &fftBuffer[0], numSamples, 1 );

	normalizedSamples.resize( numSamples );
	for ( std::size_t i=0; i<numSamples; ++i )
	{
		normalizedSamples[i] = fftBuffer[i*2]  / numSamples;
	}
}

void WaveForm::doStuff( NormalizedSamples& normalizedSamples )
{
	std::size_t numSamples = normalizedSamples.size();
	std::vector<float> fftBuffer;
	fftBuffer.resize( 2 * numSamples );
	
	long logN = (long)(log((float)numSamples)/log(2.)+.5);
	if ( 1 << logN != numSamples )
		return;

	for ( std::size_t i=0; i<numSamples; ++i )
	{
		fftBuffer[i*2+0] = normalizedSamples[i];
		fftBuffer[i*2+1] = 0;
	}
	smbFft( &fftBuffer[0], numSamples, -1 );

//	if ( numSamples>5 )
//		for ( std::size_t i=0; i<5; ++i )
//			fftBuffer[i] = 0.f;

	//for ( std::size_t i=fftBuffer.size()/2; i<fftBuffer.size(); ++i )
	//	fftBuffer[i] = 0.f;

	smbFft( &fftBuffer[0], numSamples, +1 );
	for ( std::size_t i=0; i<numSamples; ++i )
	{
		normalizedSamples[i] = fftBuffer[i*2] / numSamples;
	}
}


/*
 FFT routine, (C)1996 S.M.Bernsee. Sign = -1 is FFT, 1 is iFFT (inverse)
 
 Fills fftBuffer[0...2*fftFrameSize-1] with the Fourier transform of the time 
 domain data in fftBuffer[0...2*fftFrameSize-1]. The FFT array takes and returns
 the cosine and sine parts in an interleaved manner, ie. 
 fftBuffer[0] = cosPart[0], fftBuffer[1] = sinPart[0], asf. fftFrameSize must 
 be a power of 2. It expects a complex input signal (see footnote 2), ie. when 
 working with 'common' audio signals our input signal has to be passed as 
 {in[0],0.,in[1],0.,in[2],0.,...} asf. In that case, the transform of the 
 frequencies of interest is in fftBuffer[0...fftFrameSize].
*/
void WaveForm::smbFft(float *fftBuffer, long fftFrameSize, long sign) 
{
    float wr, wi, arg, *p1, *p2, temp;
    float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
    long i, bitm, j, le, le2, k, logN;
    logN = (long)(log((float)fftFrameSize)/log(2.)+.5);
	
    for (i = 2; i < 2*fftFrameSize-2; i += 2) {
        for (bitm = 2, j = 0; bitm < 2*fftFrameSize; bitm <<= 1) {
            if (i & bitm) j++;
            j <<= 1;
		}
		if (i < j) {
			p1 = fftBuffer+i; p2 = fftBuffer+j;
			temp = *p1; *(p1++) = *p2;
			*(p2++) = temp; temp = *p1;
			*p1 = *p2; *p2 = temp;
		}
	}
	
	for (k = 0, le = 2; k < logN; k++) {
		le <<= 1;
		le2 = le>>1;
		ur = 1.0;
		ui = 0.0;
		arg = M_PI / (le2>>1);
		wr = cos(arg);
		wi = sign*sin(arg);
		for (j = 0; j < le2; j += 2) {
			p1r = fftBuffer+j; p1i = p1r+1;
			p2r = p1r+le2; p2i = p2r+1;
			for (i = j; i < 2*fftFrameSize; i += le) {
				tr = *p2r * ur - *p2i * ui;
				ti = *p2r * ui + *p2i * ur;
				*p2r = *p1r - tr; *p2i = *p1i - ti;
				*p1r += tr; *p1i += ti;
				p1r += le; p1i += le;
				p2r += le; p2i += le;
			}
			tr = ur*wr - ui*wi;
			ui = ur*wi + ui*wr;
			ur = tr;
		}
	}
}

float WaveForm::calculatePowerLevel( const FFTBins& fftBins )
{
	float powerLevel = 0.f;
	for ( std::size_t j=0; j<fftBins.size(); ++j )
	{
		float module = sqrt( fftBins[j].first*fftBins[j].first + fftBins[j].second*fftBins[j].second );
		powerLevel += module;
	}
	powerLevel /= fftBins.size();
	return powerLevel;
}
